

import java.nio.ByteBuffer;
import java.util.ArrayList;
import java.nio.ByteOrder;
import java.nio.DoubleBuffer;
import java.nio.IntBuffer;
import java.nio.FloatBuffer;
import org.lwjgl.*;
import org.lwjgl.input.Keyboard;
import org.lwjgl.input.Mouse;
import org.lwjgl.opengl.*;
import org.lwjgl.util.Color;
import org.lwjgl.util.glu.GLU;

import static org.lwjgl.opengl.GL11.*;
import static org.lwjgl.opengl.GL12.*;
import static org.lwjgl.opengl.GL13.*;
import static org.lwjgl.opengl.GL20.*;


class Garbage3
{
	double steplength = GLTestingAgain.steplength;
	double g = -160;
	double dragcoefficient = .000015;
	double dragcoefficient2 = .00025;
	double speed, acceleration, drag;
	double[] location = new double[3];
	double[] direction;
	double height;

	public Garbage3(double xx, double yy, double zz, double s, double[] dir)
	{
		location[0] = xx;
		location[1] = yy;
		location[2] = zz;
		speed = s;
		direction = dir;
		height = yy;
	}

	public double updateSpeed(int index, ArrayList<double[]> list)
	{
		acceleration = list.get(index)[1] * g;
		speed += acceleration;
		drag = (speed * speed) * dragcoefficient +speed*dragcoefficient2;
		if (speed > 0)
		{
			speed -= drag;
		} else
		{
			speed += drag;
		}

		return speed;
	}

}

public class GLTestingDemo extends Thread
{
	  static FloatBuffer mat_specular = ByteBuffer.allocateDirect(400).asFloatBuffer().put(new float[]{1,1,1,1});
	  static  FloatBuffer mat_shininess = ByteBuffer.allocateDirect(400).asFloatBuffer().put(new float[]{50});
	  static   FloatBuffer light_position = ByteBuffer.allocateDirect(400).asFloatBuffer().put(new float[]{1,-10,1,0});
	    
	    
	    static   FloatBuffer light_ambient = ByteBuffer.allocateDirect(400).asFloatBuffer().put(new float[]{ 0.0f, 0.0f, 0.0f, 1.0f });
	    static    FloatBuffer light_diffuse = ByteBuffer.allocateDirect(400).asFloatBuffer().put(new float[]{1.0f, 1.0f, 1.0f, 1.0f});
	    static    FloatBuffer light_specular = ByteBuffer.allocateDirect(400).asFloatBuffer().put(new float[]{1,1,1,1});
	static double[] theOrigin = { 0, 0, 0 };
	static double[] notTheOrigin = { 0, 100, 0 };
	static final double steplength = 0.01;
	static boolean freeCamera = true;
	static double speedBoost = .5;

	static double rotateSpeed = 1;
	static double movementSpeed = .2;
	static boolean gridEnabled = false;
	static boolean MOVE = false;
	static double[] currentTrackLocation = { 0, 0, 0 };
	static double[] currentTrackDirection = { 0, 0 };
	static Garbage theCart = new Garbage(0, 6, 3, 5, currentTrackDirection.clone());
	static int realPathIterator = 0;
	static float shit, shack;
	static float[] red = { 1, 0, 0 };
	static float[] blue = { 0, 0, 1 };
	static float[] green = { 0, 1, 0 };
	static float[] yellow = { 1, 1, 0 };
	static float[] white = { 1, 1, 1 };
	static ByteBuffer shitState = ByteBuffer.allocateDirect(128);
	static DoubleBuffer lastState = shitState.asDoubleBuffer();
	static float n = 0;
	static double piover180 = Math.PI / 180.;
	static boolean discrete = false;

	static double[] sineTable = buildSineTable();
	static double[] cosineTable = buildCosineTable();

	static double scalex, scaley, X, Y, Z;
	static float pitch, yaw, roll;

	static double[] zpos = { 0, 0, 1 };
	static double[] zneg = { 0, 0, -1 };
	static double[] xpos = { 1, 0, 0 };
	static double[] xneg = { -1, 0, 0 };

	static boolean keyPressed = false;
	

	static ArrayList<double[]> theRealPathDirectionVectors = new ArrayList<double[]>();
	static ArrayList<double[]> theRealPathDirectionVectors2 = new ArrayList<double[]>();
	static ArrayList<double[]> theNormalVectors = new ArrayList<double[]>();
	static ArrayList<double[]> theAbnormalVectors = new ArrayList<double[]>();
	
	static ArrayList<double[]> theRealPath= buildPath();

	
	public ArrayList<ArrayList<double[]>> getCoaster() {
		ArrayList<ArrayList<double[]>> theCoasterComplete = new ArrayList<ArrayList<double[]>>();
		theCoasterComplete.add(theRealPath);
		theCoasterComplete.add(theRealPathDirectionVectors);
		theCoasterComplete.add(theRealPathDirectionVectors2);
		theCoasterComplete.add(theNormalVectors);
		theCoasterComplete.add(theAbnormalVectors);
		return theCoasterComplete;
	}
	
	public void loadCoaster(String coasterName) {
		ArrayList<ArrayList<double[]>> theCoasterComplete = GameManager.GM.database.loadCoaster(coasterName);
		theRealPath = theCoasterComplete.get(0);
		theRealPathDirectionVectors = theCoasterComplete.get(1);
		theRealPathDirectionVectors2 = theCoasterComplete.get(2);
		theNormalVectors = theCoasterComplete.get(3);
		theAbnormalVectors = theCoasterComplete.get(4);
		for(double[] d : theRealPath){
			//System.out.println("REAL POINT: "+d[0]+","+d[1]+","+d[2]);
		}
	}
	
	public void toggleGrid() {
		gridEnabled = !gridEnabled;
	}
	
	public void setMove(boolean move) {
		MOVE = move;
	}

	public boolean getMOVE() {
		return MOVE;
	}
	
	public void setFreeCamera(boolean fc) {
		freeCamera = fc;
	}
	
	public void run()
	{

		try
		{

			Display.setDisplayModeAndFullscreen(new DisplayMode(1280, 1024));
			Display.create();

		} catch (Exception e)
		{
		}
		//LIGHTNING
	 
	    


//    glMaterial(GL_FRONT, GL_SPECULAR, mat_specular);
//	    glMaterial(GL_FRONT, GL_SHININESS, mat_shininess);
	   // glLight(GL_LIGHT0, GL_POSITION, light_position);
	   
	   // glLight(GL_LIGHT0, GL_AMBIENT, light_ambient);
	   // glLight(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
	   // glLight(GL_LIGHT0, GL_SPECULAR, light_specular);
	  

	 //   glEnable(GL_LIGHTING);
	   // glEnable(GL_LIGHT0);
	    glEnable(GL_COLOR_MATERIAL);
	    glDepthFunc(GL_LEQUAL);
	    glEnable(GL_DEPTH_TEST);
	    glShadeModel(GL_SMOOTH);
		//LIGHTNING
		glMatrixMode(GL_PROJECTION);
		glLoadIdentity();
		glFrustum(-1.0, 1.0, -1.0, 1.0, 1.2, 2000.0);

		glMatrixMode(GL_MODELVIEW);
		glViewport(0, 0, 1280, 1024);
		
		

		while (!Display.isCloseRequested())
		{

			try
			{
				display();
			} catch (Exception e)
			{
				e.printStackTrace();
				System.exit(1);
			}

			// You see this line?
			Display.update();
			// Don't you DARE remove it!
			// Doing so will lock up the entire machine!
		}
		Display.destroy();
		
		System.out.println("DIRECTONSDIRECTONSDIRECTONSDIRECTONSDIRECTONSDIRECTONSDIRECTONSDIRECTONS");
		for(double[] s : theRealPathDirectionVectors2){
			System.out.println("DIRECTONSDIRECTONSDIRECTONSDIRECTONSDIRECTONSDIRECTONSDIRECTONSDIRECTONS");
			System.out.println(s[0]+",  "+s[1]+", "+s[2]);
	}}

	static ArrayList<double[]> buildPath()
	{
		ArrayList<double[]> path = new ArrayList<double[]>();

		// path.addAll(buildStraightPiece(1, new double[] { -5, 0, 0 }, new
		// double[] { 0, 0, 1 }));

		path.addAll(buildAnyPiece(4, currentTrackLocation, 0, 0));

		return path;
	}

	static void forward()
	{
		theRealPath.addAll(buildAnyPiece(.4, currentTrackLocation, 0, 0));
	}

	static void left()
	{
		theRealPath.addAll(buildAnyPiece(.2, currentTrackLocation, 0, 3));
	}

	static void right()
	{
		theRealPath.addAll(buildAnyPiece(.2, currentTrackLocation, 0, -3));
	}

	static void up()
	{
		theRealPath.addAll(buildAnyPiece(.2, currentTrackLocation, 1, 0));
	}

	static void down()
	{
		theRealPath.addAll(buildAnyPiece(.2, currentTrackLocation, -1, 0));
	}
	
	static void discreteforward()
	{
		theRealPath.addAll(buildAnyPiece(4, currentTrackLocation, 0, 0));
	}

	static void discreteleft()
	{
		theRealPath.addAll(buildAnyPiece(2, currentTrackLocation, 0, 30));
	}

	static void discreteright()
	{
		theRealPath.addAll(buildAnyPiece(2, currentTrackLocation, 0, -30));
	}

	static void discreteup()
	{
		theRealPath.addAll(buildAnyPiece(2, currentTrackLocation, 10, 0));
	}

	static void discretedown()
	{
		theRealPath.addAll(buildAnyPiece(2, currentTrackLocation, -10, 0));
	}

	static void drawPath(ArrayList<double[]> path, float[] color)
	{
		double pathWidth = .5;
		double pathThickness = .2;
		glColor3f(color[0], color[1], color[2]);
		// glBegin(GL_QUADS);
		for (int i = 1; i < path.size(); i++)
		{
			//if (i%80 == 0 || i%80 == 1 || i%80 == 2 || i%80 == 3 || i%80 == 4 || i%80 == 5 || i%80 == 6 || i%80 == 7) 
			if (i % 80 < 40){
				glColor3f(1f, 1f, 1f);
			} else {
				glColor3f(color[0], color[1], color[2]);
			}

			glBegin(GL_QUADS);

			double[] firstpoint = path.get(i - 1);
			double[] secondpoint = path.get(i);
		//	double[] direction = theRealPathDirectionVectors.get(i - 1);
			double[] widthDir = theNormalVectors.get(i-1);
			double[] victor = theAbnormalVectors.get(i-1);
			// glVertex3d(firstpoint[0]+pathWidth*direction[2], firstpoint[1],
			// firstpoint[2]+pathWidth*direction[0]);
			// glVertex3d(secondpoint[0]+pathWidth*direction[2], secondpoint[1],
			// secondpoint[2]+pathWidth*direction[0]);
			// glVertex3d(secondpoint[0]-pathWidth*direction[2], secondpoint[1],
			// secondpoint[2]-pathWidth*direction[0]);
			// glVertex3d(firstpoint[0]-pathWidth*direction[2], firstpoint[1],
			// firstpoint[2]-pathWidth*direction[0]);
//			double zToXRatio = 0;
//			double[] widthDir = { 0, 0 };
//			if (direction[0] != 0)
//			{
//				zToXRatio = (-1.0) * direction[2] / direction[0];
//				widthDir[0] = (-1.0) * zToXRatio;
//				widthDir[1] = -1.0;
//
//				double normalize = Math.sqrt(Math.pow(widthDir[0], 2) + 1);
//
//				widthDir[0] = widthDir[0] / normalize;
//				widthDir[1] = widthDir[1] / normalize;
//			} else
//			{
//				zToXRatio = -1;
//				widthDir[0] = -1;
//			}

			glVertex3d(firstpoint[0] + pathWidth * widthDir[0], firstpoint[1], firstpoint[2] + pathWidth * widthDir[1]);

			glVertex3d(secondpoint[0] + pathWidth * widthDir[0], secondpoint[1], secondpoint[2] + pathWidth * widthDir[1]);
		
			glVertex3d(secondpoint[0] - pathWidth * widthDir[0], secondpoint[1], secondpoint[2] - pathWidth * widthDir[1]);
		
			glVertex3d(firstpoint[0] - pathWidth * widthDir[0], firstpoint[1], firstpoint[2] - pathWidth * widthDir[1]);


			/*
			 * glVertex3d(firstpoint[0]+pathWidth*widthDir[0], firstpoint[1] -
			 * pathThickness, firstpoint[2]+pathWidth*widthDir[1]);
			 * glVertex3d(secondpoint[0]+pathWidth*widthDir[0], secondpoint[1] -
			 * pathThickness, secondpoint[2]+pathWidth*widthDir[1]);
			 * glVertex3d(secondpoint[0]-pathWidth*widthDir[0], secondpoint[1] -
			 * pathThickness, secondpoint[2]-pathWidth*widthDir[1]);
			 * glVertex3d(firstpoint[0]-pathWidth*widthDir[0], firstpoint[1] -
			 * pathThickness, firstpoint[2]-pathWidth*widthDir[1]);
			 * 
			 * glVertex3d(firstpoint[0]+pathWidth*widthDir[0], firstpoint[1],
			 * firstpoint[2]+pathWidth*widthDir[1]);
			 * glVertex3d(secondpoint[0]+pathWidth*widthDir[0], secondpoint[1],
			 * secondpoint[2]+pathWidth*widthDir[1]);
			 * glVertex3d(secondpoint[0]+pathWidth*widthDir[0], secondpoint[1] -
			 * pathThickness, secondpoint[2]+pathWidth*widthDir[1]);
			 * glVertex3d(firstpoint[0]+pathWidth*widthDir[0], firstpoint[1] -
			 * pathThickness, firstpoint[2]+pathWidth*widthDir[1]);
			 * 
			 * glVertex3d(firstpoint[0]-pathWidth*widthDir[0],
			 * firstpoint[1]-pathThickness,
			 * firstpoint[2]-pathWidth*widthDir[1]);
			 * glVertex3d(secondpoint[0]-pathWidth*widthDir[0],
			 * secondpoint[1]-pathThickness,
			 * secondpoint[2]-pathWidth*widthDir[1]);
			 * glVertex3d(secondpoint[0]-pathWidth*widthDir[0], secondpoint[1],
			 * secondpoint[2]-pathWidth*widthDir[1]);
			 * glVertex3d(firstpoint[0]-pathWidth*widthDir[0], firstpoint[1],
			 * firstpoint[2]-pathWidth*widthDir[1]);
			 * 
			 * glVertex3d(firstpoint[0]+pathWidth*widthDir[0], firstpoint[1],
			 * firstpoint[2]+pathWidth*widthDir[1]);
			 * glVertex3d(secondpoint[0]+pathWidth*widthDir[0], secondpoint[1],
			 * secondpoint[2]+pathWidth*widthDir[1]);
			 * glVertex3d(secondpoint[0]-pathWidth*widthDir[0], secondpoint[1],
			 * secondpoint[2]-pathWidth*widthDir[1]);
			 * glVertex3d(firstpoint[0]-pathWidth*widthDir[0], firstpoint[1],
			 * firstpoint[2]-pathWidth*widthDir[1]);
			 * 
			 * glVertex3d(firstpoint[0]+pathWidth*widthDir[0], firstpoint[1],
			 * firstpoint[2]+pathWidth*widthDir[1]);
			 * glVertex3d(secondpoint[0]+pathWidth*widthDir[0], secondpoint[1],
			 * secondpoint[2]+pathWidth*widthDir[1]);
			 * glVertex3d(secondpoint[0]-pathWidth*widthDir[0], secondpoint[1],
			 * secondpoint[2]-pathWidth*widthDir[1]);
			 * glVertex3d(firstpoint[0]-pathWidth*widthDir[0], firstpoint[1],
			 * firstpoint[2]-pathWidth*widthDir[1]);
			 */

			glEnd();
			
			glColor3f(1f, 0f, 1f);
			
			glBegin(GL_LINES);
			
			glVertex3d(firstpoint[0], firstpoint[1], firstpoint[2]);
			glVertex3d(secondpoint[0], secondpoint[1], secondpoint[2]);
			
			glEnd();
			/*glColor3f(1f, 1f, 1f);
			glBegin(GL_LINES);
			
			glVertex3d(firstpoint[0], firstpoint[1], firstpoint[2]);
			glVertex3d(firstpoint[0] + widthDir[0], firstpoint[1], firstpoint[2] + widthDir[1]);

			glEnd();*/
			//if (i % 10 == 0) {
			//glColor3f(1f, 1f, 1f);
			//glBegin(GL_QUADS);

			//glVertex3d(firstpoint[0] + pathWidth * widthDir[0], firstpoint[1], firstpoint[2] + pathWidth * widthDir[1]);
			//glVertex3d(secondpoint[0] + pathWidth * widthDir[0], secondpoint[1], secondpoint[2] + pathWidth * widthDir[1]);

			//glVertex3d(secondpoint[0] + pathWidth * widthDir[0], secondpoint[1], secondpoint[2] + pathWidth * widthDir[1]);
			//glVertex3d(secondpoint[0] - pathWidth * widthDir[0], secondpoint[1], secondpoint[2] - pathWidth * widthDir[1]);

			//glVertex3d(secondpoint[0] - pathWidth * widthDir[0], secondpoint[1], secondpoint[2] - pathWidth * widthDir[1]);
			//glVertex3d(firstpoint[0] - pathWidth * widthDir[0], firstpoint[1], firstpoint[2] - pathWidth * widthDir[1]);

			//glVertex3d(firstpoint[0] + pathWidth * widthDir[0], firstpoint[1], firstpoint[2] + pathWidth * widthDir[1]);
			//glVertex3d(firstpoint[0] - pathWidth * widthDir[0], firstpoint[1], firstpoint[2] - pathWidth * widthDir[1]);*/

			//glEnd();
			//}
			// dummy comment
			/*
			 * glColor3f(1f, 1f, 1f); glBegin(GL_LINES);
			 * glVertex3d(firstpoint[0]+pathWidth*direction[2], firstpoint[1],
			 * firstpoint[2]+pathWidth*direction[0]);
			 * glVertex3d(secondpoint[0]+pathWidth*direction[2], secondpoint[1],
			 * secondpoint[2]+pathWidth*direction[0]); glEnd();
			 * glBegin(GL_LINES);
			 * glVertex3d(secondpoint[0]+pathWidth*direction[2], secondpoint[1],
			 * secondpoint[2]+pathWidth*direction[0]);
			 * glVertex3d(secondpoint[0]-pathWidth*direction[2], secondpoint[1],
			 * secondpoint[2]-pathWidth*direction[0]); glEnd();
			 * glBegin(GL_LINES);
			 * glVertex3d(secondpoint[0]-pathWidth*direction[2], secondpoint[1],
			 * secondpoint[2]-pathWidth*direction[0]);
			 * glVertex3d(firstpoint[0]-pathWidth*direction[2], firstpoint[1],
			 * firstpoint[2]-pathWidth*direction[0]); glEnd();
			 * glBegin(GL_LINES);
			 * glVertex3d(firstpoint[0]+pathWidth*direction[2], firstpoint[1],
			 * firstpoint[2]+pathWidth*direction[0]);
			 * glVertex3d(firstpoint[0]-pathWidth*direction[2], firstpoint[1],
			 * firstpoint[2]-pathWidth*direction[0]); glEnd();
			 */

		}
		glColor3f(0.0f, 0.0f, 0.0f);
//		for (int i = 1; i < path.size(); i++)
//		{
//			glBegin(GL_LINES);
//
//			double[] firstpoint = path.get(i - 1);
//			double[] secondpoint = path.get(i);
//			double[] direction = theRealPathDirectionVectors.get(i - 1);
//			glVertex3d(firstpoint[0], firstpoint[1], firstpoint[2]);
//			glVertex3d(secondpoint[0], secondpoint[1], secondpoint[2]);
//
//			glEnd();
//
//		}

		// for (double[] p : path)
		// {
		// //////glVertex3d(p[0], p[1], p[2]);
		// }

		// glEnd();
	}

	static void display() throws InterruptedException
	{		
		glMatrixMode(GL_MODELVIEW);
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		glLoadIdentity();

		if (Mouse.isButtonDown(0))
		{
			X = (Mouse.getX() - Display.getDisplayMode().getWidth() / 2.0) / 100.0;
			Y = (Mouse.getY() - Display.getDisplayMode().getWidth() / 2.0) / 100.0;

		}
		// Does not handle roll!
		if (Keyboard.isKeyDown(Keyboard.KEY_UP))
		{

			Z += movementSpeed * Math.cos(piover180 * yaw);
			Y += movementSpeed * Math.sin(piover180 * pitch);
			X -= movementSpeed * Math.sin(piover180 * yaw);

		}
		// Does not handle roll!
		if (Keyboard.isKeyDown(Keyboard.KEY_DOWN))
		{
			Z -= movementSpeed * Math.cos(piover180 * yaw);
			Y -= movementSpeed * Math.sin(piover180 * pitch);
			X += movementSpeed * Math.sin(piover180 * yaw);

		}
		if (Keyboard.isKeyDown(Keyboard.KEY_RIGHT))
		{
			yaw += rotateSpeed;
			// System.out.println(yaw+"y");
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_LEFT))
		{
			yaw -= rotateSpeed;
			// System.out.println(yaw+"y");
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_1))
		{
			pitch += rotateSpeed;
			// System.out.println(pitch+"p");
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_4))
		{
			pitch -= rotateSpeed;
			// System.out.println(pitch+"p");
		}
		if (discrete == true)
		{
			if (Keyboard.isKeyDown(Keyboard.KEY_SPACE))
			{
				if (keyPressed == false)
				{
					forward();
				}
				keyPressed = true;
			} else if (Keyboard.isKeyDown(Keyboard.KEY_W))
			{
				if (keyPressed == false)
				{
					up();
				}
				keyPressed = true;
			} else if (Keyboard.isKeyDown(Keyboard.KEY_S))
			{
				if (keyPressed == false)
				{
					down();
				}
				keyPressed = true;
			} else if (Keyboard.isKeyDown(Keyboard.KEY_A))
			{
				if (keyPressed == false)
				{
					left();
				}
				keyPressed = true;
			} else if (Keyboard.isKeyDown(Keyboard.KEY_D))
			{
				if (keyPressed == false)
				{
					right();
				}
				keyPressed = true;
			} else
			{
				keyPressed = false;
			}
		} else
		{
			if (Keyboard.isKeyDown(Keyboard.KEY_SPACE))
			{
				forward();
			}
			if (Keyboard.isKeyDown(Keyboard.KEY_W))
			{
				up();
			}
			if (Keyboard.isKeyDown(Keyboard.KEY_S))
			{
				down();
			}
			if (Keyboard.isKeyDown(Keyboard.KEY_A))
			{
				left();
			}
			if (Keyboard.isKeyDown(Keyboard.KEY_D))
			{
				right();
			}
		}

		if (Keyboard.isKeyDown(Keyboard.KEY_G))
		{
			gridEnabled = !gridEnabled;
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_T))
		{
			discrete = !discrete;
			System.out.println(discrete);
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_M))
		{
			MOVE = !MOVE;
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_ADD))
		{
			theCart.g -= 1;
			System.out.println("GRAVITY = -" + theCart.g);
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_SUBTRACT))
		{
			theCart.g += 1;
			System.out.println("GRAVITY = " + theCart.g);
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_I))
		{
			movementSpeed += .01;
			System.out.println("MOVEMENT SPEED = " + movementSpeed);
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_U))
		{
			movementSpeed -= .01;
			System.out.println("MOVEMENT SPEED = " + movementSpeed);
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_C))
		{
			rotateSpeed += .01;
			System.out.println("ROTATION SPEED = " + rotateSpeed);
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_X))
		{
			rotateSpeed -= .01;
			System.out.println("ROTATION SPEED = " + rotateSpeed);
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_B))
		{
			theCart.speed /= 1.15;
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_NUMPAD1))
		{
			theCart.speed += speedBoost;
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_NUMPAD2))
		{
			theCart.speed -= speedBoost;
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_NUMPAD7))
		{
			speedBoost += .01;
			System.out.println("SPEEDBOOST = " + speedBoost);
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_NUMPAD8))
		{
			speedBoost -= .01;
			System.out.println("SPEEDBOOST = " + speedBoost);
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_F))
		{
			freeCamera = !freeCamera;
			X = -theCart.location[0]; 
			Y= -(theCart.location[1] + 1); 
			Z= -theCart.location[2];
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_NUMPAD9))
		{
			notTheOrigin[1] -= 1;
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_NUMPAD6))
		{
			notTheOrigin[1] += 1;
		}
		
		if(Keyboard.isKeyDown(Keyboard.KEY_3))
		{
			roll +=rotateSpeed;
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_2))
		{
			roll -=rotateSpeed;
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_SEMICOLON))
		{
			roll = 0;
		}
		

		// drawCrosshair(white);

		double dir[] = theRealPathDirectionVectors2.get(realPathIterator);



		if (freeCamera)
		{		
			glRotatef(roll,0,0,1);
			glRotatef(pitch,1,0,0);
			glRotatef(yaw,0,1,0);

			glTranslated(X, Y, Z);
		} else
		{ //on the z axis to start
			glRotatef(roll,0,0,1);
			glRotatef(-(float) dir[0], 1, 0, 0);
			glRotatef(180 - (float) dir[1], 0, 1, 0);
			//System.out.println(theCart.location[0]);
			
			double angleAboutX = -dir[0];
			double angleAboutY = 180 - dir[1];
			//System.out.println("angleAboutY" + angleAboutY + "cos" + Math.cos(angleAboutY));
//			double yTranslation = Math.cos(angleAboutX);
//			double zTranslation = (-1.0) * Math.cos(angleAboutY) * Math.sin(angleAboutX); //
//			double xTranslation = Math.sin(angleAboutY) * Math.sin(angleAboutX);
			double yTranslation = cosine(angleAboutX);
			double zTranslation = (-1.0) * cosine(angleAboutY) * sine(angleAboutX); //
			double xTranslation = sine(angleAboutY) * sine(angleAboutX);
			glTranslated(-(theCart.location[0] + xTranslation), -(theCart.location[1] + yTranslation), -(theCart.location[2] + zTranslation));

			//glTranslated(-theCart.location[0], -(theCart.location[1] + 1), -theCart.location[2]);
			//System.out.println(theCart.location[1] + " " + theAbnormalVectors.get(realPathIterator)[1] + " " + -(theCart.location[1] - theAbnormalVectors.get(realPathIterator)[1]) + " " + -(theCart.location[1] + 1));
			//glTranslated(-theCart.location[0], -(theCart.location[1] - theAbnormalVectors.get(realPathIterator)[1]), -theCart.location[2]);
			
			//glTranslated(-theCart.location[0], -1.0, -theCart.location[2]);
		}
		// This is important otherwise your motion is too fast.
		Thread.sleep(8);

		drawCube(0, 0, -4, red);
		drawCube(4, 0, 0, blue);
		drawCube(0, 0, 4, green);
		drawCube(-4, 0, 0, yellow);

		drawCube(0, shit, 0, white);

		drawSineWave();
		drawCosineWave();
		drawPath(theRealPath, green);

		if (gridEnabled)
		{
			drawGrid(300, -5, 300, yellow, 7, theOrigin);
			drawGrid(300,0,300, red, 7, notTheOrigin);
		}

		if (MOVE)
		{
			if (realPathIterator < 0)
				realPathIterator = theRealPath.size() - 1;
			theCart.location = theRealPath.get(realPathIterator);
			drawCubev(theCart.location, red, 1);

			
			
			
			realPathIterator += theCart.updateSpeed(realPathIterator, theRealPathDirectionVectors);
			
			

			if (realPathIterator >= theRealPath.size())
				realPathIterator = 0;
		}
		//dogshit();

		glFlush();
	}

	static void drawCrosshair(float color[])
	{
		glColor3f(color[0], color[1], color[2]);
		glBegin(GL_LINE_STRIP);
		glVertex3f(0f, 0f, -1.5f);

		glVertex3f(1f, 0f, -1.5f);
		glColor3f(0, 1, 0);
		glVertex3f(-1f, 0f, -1.5f);
		// glVertex3f(0f,0,-2);
		glEnd();
	}

	static void drawSineWave()
	{

		glColor3f(0.0f, 0f, 1.0f);
		glBegin(GL_LINE_STRIP);
		for (int i = 0; i < 36000; i += 1)
		{
			glVertex3d(i / 3600.0, sineTable[i], 0);

		}
		glEnd();
	}

	static void drawCosineWave()
	{
		glColor3f(0.0f, .5f, 0);
		glBegin(GL_LINE_STRIP);
		for (int i = 0; i < 36000; i += 1)
		{
			glVertex3d(i / 3600.0, cosineTable[i], 0);

		}
		glEnd();
	}

	static void drawGrid(int x, int y, int z, float[] color, double resolution, double[] loc)
	{
		glColor3f(color[0], color[1], color[2]);
		for (double i = x * -1; i < x; i += resolution)
		{
			glBegin(GL_LINE_STRIP);
			glVertex3d(i + loc[0], y + loc[1], -z + loc[2]);
			glVertex3d(i + loc[0], y + loc[1], z + loc[2]);
			glEnd();
		}
		for (double i = z * -1; i < z; i += resolution)
		{
			glBegin(GL_LINE_STRIP);
			glVertex3d(-x + loc[0], y + loc[1], i + loc[2]);
			glVertex3d(x + loc[0], y + loc[1], i + loc[2]);
			glEnd();
		}
	}
	static void dogshit()
	{
		glColor3f(1,0,1);
		glBegin(GL_QUADS);
		
		glNormal3d(0, 1, 0);
		glVertex3d(-1,-1,-1);
		
		
		glVertex3d(1,-1,-1);
		glNormal3d(0, 1, 0);
		
		glVertex3d(1,-1,1);
		glVertex3d(-1,-1,1);
		glEnd();
		
	}

	// This will draw a wireframe cube of a fixed size at the location (x,y,z)
	// with color {red, green, blue}
	static void drawCubed(double x, double y, double z, float[] color)
	{
		glColor3f(color[0], color[1], color[2]);
		glBegin(GL_LINE_STRIP);
		glVertex3d(0 + x, 0 + y, 0 + z);
		glVertex3d(1 + x, 0 + y, 0 + z);
		glVertex3d(1 + x, 1 + y, 0 + z);
		glVertex3d(0 + x, 1 + y, 0 + z);

		glVertex3d(0 + x, 1 + y, -1 + z);
		glVertex3d(0 + x, 0 + y, -1 + z);
		glVertex3d(1 + x, 0 + y, -1 + z);
		glVertex3d(1 + x, 1 + y, -1 + z);
		glVertex3d(0 + x, 1 + y, -1 + z);

		glVertex3d(0 + x, 1 + y, 0 + z);
		glVertex3d(0 + x, 0 + y, 0 + z);
		glVertex3d(0 + x, 0 + y, -1 + z);
		glVertex3d(1 + x, 0 + y, -1 + z);
		glVertex3d(1 + x, 0 + y, 0 + z);
		glVertex3d(1 + x, 1 + y, 0 + z);
		glVertex3d(1 + x, 1 + y, -1 + z);
		glEnd();
	}

	static double sine(double angle)
	{
		if (angle < 0)
		{
			return sineTable[(int) (((angle * -1) % 360) * 100)] * -1;
		} else
		{
			return sineTable[(int) ((angle % 360) * 100)];
		}
	}

	static double cosine(double angle)
	{
		return cosineTable[Math.abs((int) ((angle % 360) * 100))];
	}

	static ArrayList<double[]> buildAnyPiece(double l, double[] loc, double pitch, double yaw)
	{

		double[] curDir = currentTrackDirection;
		double[] curLoc = loc;
		ArrayList<double[]> piece = new ArrayList<double[]>();

		int steps = (int) (100 * l);
		double angular_pitch_step = pitch / steps;
		double angular_yaw_step = yaw / steps;

		int i;
		for (i = 0; i < steps; i++)
		{
			piece.add(curLoc.clone());

			curDir[0] += angular_pitch_step; // THETA
			curDir[1] += angular_yaw_step; // PHI

			double deltax = steplength * cosine(curDir[0]) * sine(curDir[1]);
			double deltay = steplength * sine(curDir[0]);
			double deltaz = steplength * cosine(curDir[0]) * cosine(curDir[1]);
			theRealPathDirectionVectors.add(new double[] { deltax, deltay, deltaz });
			theRealPathDirectionVectors2.add(new double[] { curDir[0], curDir[1], 0});

			curLoc[0] += deltax;
			curLoc[1] += deltay;
			curLoc[2] += deltaz;
			
			
			
			double zToXRatio = 0;
			double[] widthDir = { 0, 0, 0 };
			double normalize;
			if (deltax != 0)
			{
				if ( deltaz < 0 ) {
					System.out.println("z be negative man");
				}
				if ( deltax < 0 ) {
					System.out.println("x be negative man");
				}
				zToXRatio = (-1.0) * deltaz / deltax;
				if ( zToXRatio < 0 ) {
					System.out.println("yea thats right");
				}
				widthDir[0] = (-1.0) * (zToXRatio);
				widthDir[1] = -1.0;
				
				if (deltaz < 0 && deltax > 0) {
					widthDir[0] = widthDir[0] * -1;
					widthDir[1] = 1.0;
				}
				if (deltaz > 0 && deltax > 0) {
					widthDir[0] = widthDir[0] * -1;
					widthDir[1] = 1.0;				
				}

				normalize = Math.sqrt(Math.pow(widthDir[0], 2) + 1);

				widthDir[0] = widthDir[0] / normalize;
				widthDir[1] = widthDir[1] / normalize;
			} else
			{
				zToXRatio = -1;
				widthDir[0] = -1;
			}
			
			theNormalVectors.add(widthDir.clone());
			double[] victor = new double[]{deltay*widthDir[1], deltaz*widthDir[0] - deltax*widthDir[1], -(deltay*widthDir[0]) };
			
			normalize = Math.sqrt(Math.pow(victor[0], 2) + Math.pow(victor[1], 2) + Math.pow(victor[2], 2));
			victor[0] = victor[0] / normalize;
			victor[1] = victor[1] / normalize;
			victor[2] = victor[2] / normalize;
			theAbnormalVectors.add(victor);
			
			

			
		}	

		currentTrackLocation = curLoc;
		currentTrackDirection = curDir;

		return piece;
	}

	static void drawCube(float x, float y, float z, float[] color)
	{
		glColor3f(color[0], color[1], color[2]);
		glBegin(GL_LINE_STRIP);
		glVertex3f(0 + x, 0 + y, 0 + z);
		glVertex3f(1 + x, 0 + y, 0 + z);
		glVertex3f(1 + x, 1 + y, 0 + z);
		glVertex3f(0 + x, 1 + y, 0 + z);

		glVertex3f(0 + x, 1 + y, -1 + z);
		glVertex3f(0 + x, 0 + y, -1 + z);
		glVertex3f(1 + x, 0 + y, -1 + z);
		glVertex3f(1 + x, 1 + y, -1 + z);
		glVertex3f(0 + x, 1 + y, -1 + z);

		glVertex3f(0 + x, 1 + y, 0 + z);
		glVertex3f(0 + x, 0 + y, 0 + z);
		glVertex3f(0 + x, 0 + y, -1 + z);
		glVertex3f(1 + x, 0 + y, -1 + z);
		glVertex3f(1 + x, 0 + y, 0 + z);
		glVertex3f(1 + x, 1 + y, 0 + z);
		glVertex3f(1 + x, 1 + y, -1 + z);
		glEnd();

	}

	static void drawCubev(double[] xyz, float[] color, double scale)
	{
		glColor3f(color[0], color[1], color[2]);
		glBegin(GL_LINE_STRIP);
		glVertex3d(0 + xyz[0], 0 + xyz[1], 0 + xyz[2]);
		glVertex3d(scale + xyz[0], 0 + xyz[1], 0 + xyz[2]);
		glVertex3d(scale + xyz[0], scale + xyz[1], 0 + xyz[2]);
		glVertex3d(0 + xyz[0], scale + xyz[1], 0 + xyz[2]);

		glVertex3d(0 + xyz[0], scale + xyz[1], -scale + xyz[2]);
		glVertex3d(0 + xyz[0], 0 + xyz[1], -scale + xyz[2]);
		glVertex3d(scale + xyz[0], 0 + xyz[1], -scale + xyz[2]);
		glVertex3d(scale + xyz[0], scale + xyz[1], -scale + xyz[2]);
		glVertex3d(0 + xyz[0], scale + xyz[1], -scale + xyz[2]);

		glVertex3d(0 + xyz[0], scale + xyz[1], 0 + xyz[2]);
		glVertex3d(0 + xyz[0], 0 + xyz[1], 0 + xyz[2]);
		glVertex3d(0 + xyz[0], 0 + xyz[1], -scale + xyz[2]);
		glVertex3d(scale + xyz[0], 0 + xyz[1], -scale + xyz[2]);
		glVertex3d(scale + xyz[0], 0 + xyz[1], 0 + xyz[2]);
		glVertex3d(scale + xyz[0], scale + xyz[1], 0 + xyz[2]);
		glVertex3d(scale + xyz[0], scale + xyz[1], -scale + xyz[2]);
		glEnd();

	}

	public static double[] buildSineTable()
	{
		double piover180 = (Math.PI / 180.0);
		double[] Table = new double[36001];
		int intdegree = 0;
		for (double degree = 0; degree < 360; degree += 0.01)
		{

			Table[intdegree] = Math.sin(piover180 * degree);

			intdegree++;
		}
		return Table;
	}

	public static double[] buildCosineTable()
	{
		double piover180 = (Math.PI / 180.0);
		double[] cosineTable = new double[36001];
		int intdegree = 0;
		for (double degree = 0; degree < 360; degree += 0.01)
		{

			cosineTable[intdegree] = Math.cos(piover180 * degree);
			intdegree++;

		}
		return cosineTable;
	}

}